#include "stm32f10x.h"                  // Device header
#include "PWM.h"
#include "Servo.h"
#include "Delay.h"

uint8_t i;



void move_foward(void)
{
	Servo_SetAngle1(45);
	Servo_SetAngle4(135);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay_ms(500);
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Servo_SetAngle2(135);
	Servo_SetAngle3(45);
	Delay_ms(500);
}


void move_back(void)
{
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Servo_SetAngle2(45);
	Servo_SetAngle3(135);
	Delay_ms(500);
	Servo_SetAngle1(135);
	Servo_SetAngle4(45);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay_ms(500);
}

void move_left(void)
{
	Servo_SetAngle2(45);
	Servo_SetAngle3(45);
	Servo_SetAngle1(135);
	Servo_SetAngle4(135);
	Delay_ms(200);
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Delay_ms(200);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay_ms(200);
}

void move_right(void)
{
	Servo_SetAngle2(135);
	Servo_SetAngle3(135);
	Servo_SetAngle1(45);
	Servo_SetAngle4(45);
	Delay_ms(200);
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Delay_ms(200);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay_ms(200);
}	

void move_swing(void)
{
	Servo_SetAngle1(45);
	Servo_SetAngle4(135);
	Servo_SetAngle2(135);
	Servo_SetAngle3(45);
	Delay_ms(200);
	Servo_SetAngle1(135);
	Servo_SetAngle4(45);
	Servo_SetAngle2(45);
	Servo_SetAngle3(135);
	Delay_ms(200);
}

void move_stand(void)
{
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
}



void move_sit(void)
{
	for (i = 0; i < 20; i++) 
	{
		Servo_SetAngle4(90-i);
		Servo_SetAngle3(90+i);
	}
	Delay_ms(20);
	for (i = 0; i < 55; i++)
	{
		Servo_SetAngle2(90+i);
		Servo_SetAngle1(90-i);
	}
}


void move_stretch(void)
{
	for ( i = 0; i < 70; i++) 
	{
		Servo_SetAngle3(90+i);
		Servo_SetAngle1(90-i);
	}
	Delay_ms(1000);
	for (i = 0; i < 70; i++) 
	{
		Servo_SetAngle3(160-i);
		Servo_SetAngle1(20+i);
	}
	
	for (i = 0; i < 20; i++) 
	{
		Servo_SetAngle4(90+i);
		Servo_SetAngle3(90-i);
		
	}
	for (i = 0; i < 20; i++) 
	{
		Servo_SetAngle3(90-i);
		Servo_SetAngle1(90-i);
		Servo_SetAngle2(90+i);
	}
	Delay_ms(1000);
	for (i = 1; i <= 20; i++) 
	{
		Servo_SetAngle3(70+i);
	}
	Delay_ms(1000);
	Servo_SetAngle4(180);
	Delay_ms(500);
	Servo_SetAngle4(130);
	Delay_ms(500);
	Servo_SetAngle4(180);
	Delay_ms(500);
	Servo_SetAngle4(130);
	Delay_ms(500);
	Servo_SetAngle4(90);

}


void move_butt_up(void)
{
	for ( i = 0; i < 60; i++) 
	{
		Servo_SetAngle1(90);	
		Servo_SetAngle3(90-i); 
		Servo_SetAngle2(90);   
		Servo_SetAngle4(90+i); 
		Delay_ms(15);
	}	
}


void move_sleep_i(void)
{
	for ( i = 0; i < 75; i++) 
	{
		Servo_SetAngle1(90-i);
		Servo_SetAngle3(90+i);
	}
	Delay_ms(20);
	for (i = 0; i < 75; i++)
	{
		Servo_SetAngle2(90+i);
		Servo_SetAngle4(90-i);
	}
}


void move_sleep_o(void)
{
	for ( i = 0; i < 75; i++) 
	{
		Servo_SetAngle1(90+i);
		Servo_SetAngle3(90-i);
	}
	Delay_ms(20);
	for (i = 0; i < 75; i++)
	{
		Servo_SetAngle2(90-i);
		Servo_SetAngle4(90+i);
	}
}

	